﻿// Decompiled with JetBrains decompiler
// Type: DnProcessSimulateCommands.ExportToRTR.CApExportToRTRApp
// Assembly: DnProcessSimulateCommands, Version=2402.0.0.890, Culture=neutral, PublicKeyToken=null
// MVID: A885D881-AEDF-447E-852A-6F13102E089C
// Assembly location: C:\Program Files\Tecnomatix_2402\eMPower\DotNetCommands\DnProcessSimulateCommands.dll

using DnProcessSimulateCommands.ExportToRTR.Resources;
using DnProcessSimulateCommands.ExportToRTR.YAML;
using EngineeringInternalExtension;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.IO;
using System.IO.Compression;
using System.Linq;
using System.Windows.Forms;
using Tecnomatix.Engineering;
using YamlDotNet.Serialization;
using YamlDotNet.Serialization.NamingConventions;

namespace DnProcessSimulateCommands.ExportToRTR
{
    public class CApExportToRTRApp
    {
        private const string WORLD_LINK = "world";
        internal const string ROBOT_URDF = "robot.urdf";
        private const string LOG_FILE_NAME_SUFFIX = ".log";
        private const string MINIMAL_CLEARANCE = "Minimum_clearance";
        private string m_path;
        private CUiExportToRTROutputViewModel m_outputVM;
        private CUiExportToRTRContentDataHandler m_contentDataHandler;
        private CApExportToRTRUserMessages m_userMessage;
        private Dictionary<TxRobot, List<TxJoint>> m_robotExternalJoints;
        private Dictionary<string, string> m_connectedToPrev = new Dictionary<string, string>();
        private Dictionary<ITxObject, TxRobot> m_robotChildrenWith3D;
        private Dictionary<ITxObject, TxRobot> m_robotSuplementsWith3D;
        private HashSet<ITxObject> m_staticObjectsWith3D;
        private Dictionary<TxRobot, Dictionary<string, ITxObject>> m_robotPrototypeLinksWith3D;

        public CApExportToRTRApp()
        {
        }

        public CApExportToRTRApp(
          CUiExportToRTROutputViewModel outputVM,
          CUiExportToRTRContentDataHandler contentDataHandler)
        {
            this.m_outputVM = outputVM;
            this.m_contentDataHandler = contentDataHandler;
        }

        public string Export()
        {
            Cursor current = Cursor.Current;
            Cursor.Current = Cursors.WaitCursor;
            this.m_path = Path.Combine(this.m_outputVM.ExportPath, this.m_outputVM.PackageName);
            Directory.CreateDirectory(this.m_path);
            this.m_userMessage = new CApExportToRTRUserMessages(Path.Combine(this.m_path, string.Format(ExportToRTRStringTable.LOG_FILE_NAME, (object)this.m_outputVM.PackageName) + ".log"));
            string path = this.m_path;
            Rootobject root = new Rootobject()
            {
                RtrProject = new RtrProject()
                {
                    RtrYamlVersion = "4.0.0",
                    ProjectName = this.m_outputVM.PackageName
                }
            };
            this.ExportObjectsToYaml(root.RtrProject);
            this.ExportLocationsToYaml(root.RtrProject);
            if (this.m_contentDataHandler.ComputeCollisionSets)
                this.ExportCollisionSetsToYaml(root.RtrProject);
            this.YAMLWriter(root);
            this.m_userMessage.LogWriter.Close();
            if (this.m_userMessage.IsEmpty())
                this.m_userMessage.RemoveLog();
            else
                this.m_userMessage.Show();
            string str1 = Path.Combine(this.m_outputVM.ExportPath, this.m_outputVM.PackageName + ".zip");
            if (File.Exists(str1))
                File.Delete(str1);
            ZipFile.CreateFromDirectory(this.m_path, str1, CompressionLevel.Optimal, true);
            string str2 = str1;
            CApExportToRTRUtils.ResetNames();
            Cursor.Current = current;
            return str2;
        }

        private void ExportObjectsToYaml(RtrProject rtrProject)
        {
            CApExportToRTRYamlMgr.Instance.Clear();
            if (this.m_contentDataHandler.SelectedObjects.Count > 0)
            {
                CApExportToRTRExportManager rtrExportManager = new CApExportToRTRExportManager(this.m_contentDataHandler.SelectedObjects, this.m_contentDataHandler, this.m_userMessage.LogWriter);
                rtrExportManager.Export((CApExportToRTRXmlWriter)null, this.m_path, "world");
                this.m_robotChildrenWith3D = rtrExportManager.RobotChildrenWith3D;
                this.m_robotSuplementsWith3D = rtrExportManager.RobotSuplementsWith3D;
                this.m_staticObjectsWith3D = rtrExportManager.StaticObjectsWith3D;
                this.m_robotPrototypeLinksWith3D = rtrExportManager.RobotPrototypeLinksWith3D;
            }
            rtrProject.Workcell = CApExportToRTRYamlMgr.Instance.Workcell;
        }

        private void AddExtenralAxisValues(
          TxRobot robot,
          ITxRoboticLocationOperation location,
          ref List<double> jointValues)
        {
            if (!CApExportToRTRYamlMgr.Instance.ExternalJoints.ContainsKey(robot))
                return;
            TxJoint[] externalJoint = CApExportToRTRYamlMgr.Instance.ExternalJoints[robot];
            if (externalJoint.Length == 0)
                return;
            List<TxRobotExternalAxisData> externalAxisDataList = location.RobotExternalAxesData != null ? new List<TxRobotExternalAxisData>((IEnumerable<TxRobotExternalAxisData>)location.RobotExternalAxesData) : (List<TxRobotExternalAxisData>)null;
            foreach (TxJoint txJoint in externalJoint)
            {
                TxJoint joint = txJoint;
                double num = joint.CurrentValue;
                if (externalAxisDataList != null && externalAxisDataList.Exists((Predicate<TxRobotExternalAxisData>)(x => x.Joint == (ITxObject)joint)))
                    num = externalAxisDataList.Find((Predicate<TxRobotExternalAxisData>)(x => x.Joint == (ITxObject)joint)).JointValue;
                jointValues.Add(num);
            }
        }

        private void ExportLocationsToYaml(RtrProject rtrProject)
        {
            foreach (CApExportToRTRLocations exportToRtrLocations in this.SortLocationsByRobot().Values)
            {
                TxRobot robot = exportToRtrLocations.m_robot;
                RobotInstance robotInstance = CApExportToRTRYamlMgr.Instance.GetObj((ITxObject)robot) as RobotInstance;
                RoadMap roadMap = new RoadMap();
                if (robotInstance != null)
                {
                    roadMap.Preset = robotInstance.DefaultPreset;
                    roadMap.Robot = robotInstance.Name;
                    roadMap.JointLimits = robotInstance.JointLimits;
                    roadMap.ValidationFlags = new FlowFormatList<string>()
          {
            "all_direct_connections_valid_error",
            "all_targets_reachable_error",
            "all_planned_connections_valid_error"
          };
                }
                else
                    this.m_userMessage.LogWriter.WriteLine(ExportToRTRStringTable.OPERATION_NO_ROBOT_EXPORTED_WARNING_LOG);
                FlowFormatList<IIgnoreObjFlowForamt> flowFormatList = this.m_contentDataHandler.ComputeCollisionSets ? (FlowFormatList<IIgnoreObjFlowForamt>)null : this.PopulateIgnoreCollision(robot);
                foreach (ITxRoboticLocationOperation location in exportToRtrLocations.m_locations)
                {
                    CApExportToRTRLocationData toRtrLocationData;
                    if (!this.m_contentDataHandler.DataOfLocations.TryGetValue(location, out toRtrLocationData))
                    {
                        ++this.m_userMessage.LocationsInfo.InvalidCounter;
                        this.m_userMessage.LogWriter.WriteLine(string.Format(ExportToRTRStringTable.LOCATION_NO_ROBOT_EXPORTED_WARNING_LOG, (object)location.Name, (object)location.ParentRoboticOperation.Name));
                    }
                    if (robotInstance == null)
                    {
                        ++this.m_userMessage.LocationsInfo.InvalidCounter;
                        this.m_userMessage.LogWriter.WriteLine(string.Format(ExportToRTRStringTable.LOCATION_NO_ROBOT_EXPORTED_WARNING_LOG, (object)location.Name, (object)location.ParentRoboticOperation.Name));
                    }
                    else if (this.m_contentDataHandler.UnConfiguredLocations.Contains(location))
                    {
                        ++this.m_userMessage.LocationsInfo.InvalidCounter;
                        this.m_userMessage.LogWriter.WriteLine(string.Format(ExportToRTRStringTable.LOCATION_NOT_CONFIGURED_WARNING_LOG, (object)location.Name, (object)location.ParentRoboticOperation.Name, (object)robot.Name));
                    }
                    else
                    {
                        TxPoseData robotPose = toRtrLocationData.RobotPose;
                        if (this.m_contentDataHandler.UnReachableLocations.Contains(location))
                        {
                            ++this.m_userMessage.LocationsInfo.InvalidCounter;
                            this.m_userMessage.LogWriter.WriteLine(string.Format(ExportToRTRStringTable.LOCATION_NOT_VALID_ERROR_LOG, (object)robot.Name, (object)location.Name, (object)location.ParentRoboticOperation.Name));
                        }
                        else if (this.m_contentDataHandler.CollidingLocations.Contains(location))
                        {
                            string str = "";
                            if (location.ParentRoboticOperation is ITxWeldOperation roboticOperation && roboticOperation.Gun != null)
                                str = roboticOperation.Gun.Name;
                            ++this.m_userMessage.LocationsInfo.InvalidCounter;
                            this.m_userMessage.LogWriter.WriteLine(string.Format(ExportToRTRStringTable.LOCATION_COLLIDING_GUN_WARNING_LOG, (object)location.Name, (object)robot.Name, (object)str));
                        }
                        else if (robotPose != null)
                        {
                            List<double> list = robotPose.JointValues.Cast<double>().ToList<double>();
                            this.AddExtenralAxisValues(robot, location, ref list);
                            string uniqueNameForRtr = location.GetUniqueNameForRTR(true);
                            roadMap.Assignments.Add(new Assignment()
                            {
                                Target = uniqueNameForRtr,
                                JointConfig = new FlowFormatList<double>(list),
                                IgnoreCollision = flowFormatList,
                                IsWeldLoc = location is TxWeldLocationOperation
                            });
                            if (toRtrLocationData.NextLocation != null && toRtrLocationData.NextLocation.GetParameter("RRS_MOTION_TYPE") is TxRoboticIntParam parameter && parameter.Value == 2)
                                this.m_connectedToPrev.Add(toRtrLocationData.NextLocation.GetUniqueNameForRTR(true), uniqueNameForRtr);
                        }
                    }
                }
                if (robotInstance != null)
                {
                    this.GenerateConnections(roadMap);
                    rtrProject.Roadmaps.Add(roadMap);
                }
            }
            this.AddRobotObstacles(rtrProject);
        }

        private RoadMap CreateRoadmapForStaticRobot(RobotInstance staticRobot)
        {
            RoadMap roadmapForStaticRobot = (RoadMap)null;
            ITxObject key = CApExportToRTRYamlMgr.Instance.m_Tx2YamlObject.First<KeyValuePair<ITxObject, IObj>>((Func<KeyValuePair<ITxObject, IObj>, bool>)(x => x.Value == staticRobot)).Key;
            if (key != null)
            {
                using (TxPoseData currentPose = (key as TxRobot)?.CurrentPose)
                {
                    List<double> doubleList;
                    if (currentPose == null)
                    {
                        doubleList = (List<double>)null;
                    }
                    else
                    {
                        ArrayList jointValues1 = currentPose.JointValues;
                        doubleList = jointValues1 != null ? jointValues1.Cast<double>().ToList<double>() : (List<double>)null;
                    }
                    List<double> jointValues = doubleList;
                    if (CApExportToRTRYamlMgr.Instance.ExternalJoints.ContainsKey(key as TxRobot))
                        ((IEnumerable<TxJoint>)CApExportToRTRYamlMgr.Instance.ExternalJoints[key as TxRobot]).ToList<TxJoint>().ForEach((Action<TxJoint>)(x => jointValues.Add(x.CurrentValue)));
                    roadmapForStaticRobot = new RoadMap()
                    {
                        Robot = staticRobot.Name,
                        Preset = staticRobot.DefaultPreset
                    };
                    roadmapForStaticRobot.Assignments.Add(new Assignment()
                    {
                        Target = string.Format("{0}_{1}", (object)"CurrentPose", (object)key.Id.Replace(",", "_")),
                        JointConfig = new FlowFormatList<double>(jointValues)
                    });
                }
            }
            return roadmapForStaticRobot;
        }

        private void AddRobotObstacles(RtrProject rtrProject)
        {
            List<RobotInstance> robots = CApExportToRTRYamlMgr.Instance.Workcell.Robots;
            List<RoadMap> collection = new List<RoadMap>();
            foreach (RoadMap roadmap in rtrProject.Roadmaps)
            {
                foreach (RobotInstance robotInstance in robots)
                {
                    RobotInstance otherRobot = robotInstance;
                    if (otherRobot.Name != roadmap.Robot)
                    {
                        RoadMap roadmapForStaticRobot = rtrProject.Roadmaps.Find((Predicate<RoadMap>)(x => x.Robot == otherRobot.Name));
                        if (roadmapForStaticRobot == null)
                        {
                            roadmapForStaticRobot = collection.Find((Predicate<RoadMap>)(x => x.Robot == otherRobot.Name));
                            if (roadmapForStaticRobot == null)
                            {
                                roadmapForStaticRobot = this.CreateRoadmapForStaticRobot(otherRobot);
                                collection.Add(roadmapForStaticRobot);
                            }
                        }
                        if (roadmapForStaticRobot != null)
                        {
                            foreach (Dictionary<string, Connection> connection1 in roadmap.Connections)
                            {
                                foreach (Connection connection2 in connection1.Values)
                                {
                                    if (connection2 is Planned)
                                    {
                                        string target = roadmapForStaticRobot.Assignments[0].Target;
                                        (connection2 as Planned).RobotObstacles.Add(new RobotObstacle()
                                        {
                                            Robot = otherRobot.Name,
                                            Preset = otherRobot.DefaultPreset,
                                            Targets = new FlowFormatList<string>(new List<string>()
                      {
                        target
                      })
                                        });
                                    }
                                }
                            }
                        }
                    }
                }
            }
            rtrProject.Roadmaps.AddRange((IEnumerable<RoadMap>)collection);
        }

        private double ComputeMinimalDistance(ITxObject obj, ITxObject otherObj)
        {
            double distance = 0.0;
            if ((object)(obj as TxKinematicLink) != null && CApExportToRTRUtils.IsEmptyLink(obj as TxKinematicLink) || (object)(otherObj as TxKinematicLink) != null && CApExportToRTRUtils.IsEmptyLink(otherObj as TxKinematicLink))
                return distance;
            TxVector pointOnObject = new TxVector();
            TxVector pointOnOther = new TxVector();
            if (this.IsEquipmentLink(obj) && this.IsEquipmentLink(otherObj))
                ((obj as TxKinematicLink).LinkApprox()[0] as ITxLocatableObject).GetMinimalDistance((otherObj as TxKinematicLink).LinkApprox()[0] as ITxLocatableObject, out distance, out pointOnObject, out pointOnOther);
            else if (this.IsEquipmentLink(obj))
                ((obj as TxKinematicLink).LinkApprox()[0] as ITxLocatableObject).GetMinimalDistance(otherObj as ITxLocatableObject, out distance, out pointOnObject, out pointOnOther);
            else if (this.IsEquipmentLink(otherObj))
                (obj as ITxLocatableObject).GetMinimalDistance((otherObj as TxKinematicLink).LinkApprox()[0] as ITxLocatableObject, out distance, out pointOnObject, out pointOnOther);
            else
                (obj as ITxLocatableObject).GetMinimalDistance(otherObj as ITxLocatableObject, out distance, out pointOnObject, out pointOnOther);
            return distance;
        }

        private bool IsEquipmentLink(ITxObject obj)
        {
            return (object)(obj as TxKinematicLink) != null && (obj.Collection as ITxComponent).IsEquipment;
        }

        private FlowFormatList<IIgnoreObjFlowForamt> PopulateIgnoreCollision(TxRobot robot)
        {
            FlowFormatList<IIgnoreObjFlowForamt> flowList = (FlowFormatList<IIgnoreObjFlowForamt>)null;
            bool flag = true;
            if (CApExportToRTRYamlMgr.Instance.IgnoredObjects.ContainsKey(robot))
            {
                if (CApExportToRTRYamlMgr.Instance.IgnoredObjects[robot].Item1.Count > 0)
                {
                    if (flag)
                    {
                        flag = false;
                        int index = CApExportToRTRYamlMgr.Instance.IgnoredObjects[robot].Item1.Count - 1;
                        flowList = new FlowFormatList<IIgnoreObjFlowForamt>((IIgnoreObjFlowForamt)new IgnoreString(CApExportToRTRYamlMgr.Instance.GetObj(CApExportToRTRYamlMgr.Instance.IgnoredObjects[robot].Item1[index]).Name));
                        CApExportToRTRYamlMgr.Instance.IgnoredObjects[robot].Item1.RemoveAt(index);
                    }
                    if (CApExportToRTRYamlMgr.Instance.IgnoredObjects[robot].Item1.Count > 0)
                        CApExportToRTRYamlMgr.Instance.IgnoredObjects[robot].Item1.ForEach((Action<ITxObject>)(x => flowList.Add((IIgnoreObjFlowForamt)new IgnoreString(CApExportToRTRYamlMgr.Instance.GetObj(x).Name))));
                }
                Stack<ITxObject> txObjectStack = new Stack<ITxObject>((IEnumerable<ITxObject>)CApExportToRTRYamlMgr.Instance.IgnoredObjects[robot].Item2);
                int count = txObjectStack.Count;
                for (int index = 0; index < count - 1; ++index)
                {
                    ITxObject txObj = txObjectStack.Pop();
                    foreach (ITxObject txObject in txObjectStack)
                    {
                        if (this.ComputeMinimalDistance(txObj, txObject) <= 400.0)
                        {
                            FlowFormatList<string> flowFormatList = new FlowFormatList<string>(CApExportToRTRYamlMgr.Instance.GetObj(txObj).Name);
                            flowFormatList.Add(CApExportToRTRYamlMgr.Instance.GetObj(txObject).Name);
                            if (flag)
                            {
                                flag = false;
                                flowList = new FlowFormatList<IIgnoreObjFlowForamt>((IIgnoreObjFlowForamt)flowFormatList);
                            }
                            else
                                flowList.Add((IIgnoreObjFlowForamt)flowFormatList);
                        }
                    }
                }
            }
            return flowList;
        }

        private void GenerateConnections(RoadMap roadMap)
        {
            List<string> locations = new List<string>();
            foreach (Assignment assignment in roadMap.Assignments)
            {
                string str = (string)null;
                if (this.m_connectedToPrev.TryGetValue(assignment.Target, out str))
                    this.AddDirectConnetionToRoadmap(roadMap, new List<string>()
          {
            str,
            assignment.Target
          }, "cartesian");
                locations.Add(assignment.Target);
            }
            this.AddCurentPlannedConnectionToRoadmap(roadMap, locations);
        }

        private int CalculatePLanningTimeForAllConnections(int n, int t) => n * (n - 1) / 2 * t;

        private double? GetMinimumClearance()
        {
            double? minimumClearance = new double?();
            string s = CApExportToRTRUtils.ReadValueFromRegistry("Minimum_clearance");
            double result;
            if (!string.IsNullOrEmpty(s) && double.TryParse(s, out result))
                minimumClearance = new double?(result);
            return minimumClearance;
        }

        private void AddCurentPlannedConnectionToRoadmap(RoadMap roadMap, List<string> locations)
        {
            if (locations.Count <= 1)
                return;
            if (this.m_outputVM.AutoConnectAll)
            {
                this.AddConnectionToRoadmap(roadMap, locations, (Connection)new Planned()
                {
                    Dense = this.m_outputVM.DenseConnection,
                    PlanningTime = this.CalculatePLanningTimeForAllConnections(locations.Count, this.m_contentDataHandler.SegmentTime)
                });
            }
            else
            {
                for (int index = 0; index < locations.Count - 1; ++index)
                    this.AddConnectionToRoadmap(roadMap, new List<string>()
          {
            locations[index],
            locations[index + 1]
          }, (Connection)new Planned()
          {
              PlanningTime = this.m_contentDataHandler.SegmentTime
          });
            }
        }

        private void AddDirectConnetionToRoadmap(
          RoadMap roadMap,
          List<string> locations,
          string connectionType)
        {
            this.AddConnectionToRoadmap(roadMap, locations, (Connection)new DnProcessSimulateCommands.ExportToRTR.YAML.Direct()
            {
                Type = connectionType
            });
        }

        private void AddConnectionToRoadmap(RoadMap roadMap, List<string> locations, Connection con)
        {
            FlowFormatList<string> flowFormatList = new FlowFormatList<string>(locations);
            con.Targets = flowFormatList;
            roadMap.Connections.Add(new Dictionary<string, Connection>()
      {
        {
          con.GetConnectionTypeName(),
          con
        }
      });
        }

        private void YAMLWriter(Rootobject root)
        {
            SerializerBuilder serializerBuilder = new SerializerBuilder();
            serializerBuilder.ConfigureDefaultValuesHandling((DefaultValuesHandling)1);
            string str = ((BuilderSkeleton<SerializerBuilder>)serializerBuilder).WithNamingConvention(UnderscoredNamingConvention.Instance).Build().Serialize((object)root);
            using (StreamWriter text = File.CreateText(Path.Combine(this.m_path, "project.yaml")))
                text.WriteLine(str);
        }

        private void WritePrefix(CApExportToRTRXmlWriter writer)
        {
            writer.WriteStartDocument();
            writer.WriteStartElement("robot");
            writer.WriteAttributeString("name", "PS_World");
            new CApExportToRTRLink("world").Write(writer);
        }

        private void WriteSuffix(CApExportToRTRXmlWriter writer)
        {
            writer.WriteEndElement();
            writer.WriteEndDocument();
        }

        private Dictionary<TxRobot, CApExportToRTRLocations> SortLocationsByRobot()
        {
            Dictionary<TxRobot, CApExportToRTRLocations> dictionary = new Dictionary<TxRobot, CApExportToRTRLocations>();
            foreach (ITxRoboticLocationOperation selectedLocation in this.m_contentDataHandler.SelectedLocations)
            {
                TxRobot robot = selectedLocation.ParentRoboticOperation.Robot as TxRobot;
                if (!dictionary.ContainsKey(robot))
                {
                    List<TxJoint> robotExternalJoint = this.m_robotExternalJoints != null ? this.m_robotExternalJoints[robot] : (List<TxJoint>)null;
                    dictionary.Add(robot, new CApExportToRTRLocations(robot, robotExternalJoint, this.m_path));
                }
                dictionary[robot].AddLocation(selectedLocation);
            }
            return dictionary;
        }

        private void ExportCollisionSetsToYaml(RtrProject rtrProject)
        {
            TxObjectList pairList = TxApplication.ActiveDocument.CollisionRoot.PairList;
            Dictionary<string, RoadMap> roadMaps = new Dictionary<string, RoadMap>();
            rtrProject.Roadmaps.ForEach((Action<RoadMap>)(roadMap => roadMaps.Add(roadMap.Robot, roadMap)));
            foreach (TxCollisionPair collisionPair in (Collection<ITxObject>)pairList)
            {
                if (collisionPair.Active)
                    new CApExportToRTRCollisionPairExporter(collisionPair, this.m_robotSuplementsWith3D, this.m_robotChildrenWith3D, this.m_staticObjectsWith3D, this.m_robotPrototypeLinksWith3D, roadMaps, this.GetMinimumClearance()).Export();
            }
        }
    }
}
